﻿using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using NetduinoHappyHour.Hardware;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.Netduino;

namespace JohnWilson
{
    /// <summary>
    /// Class for controlling the HC-SR04 Ultrasonic Range detector
    /// Written by John E. Wilson
    /// Version 1.1 - 2012/04/03 - Corrected constructor pin documentation
    /// Free to use, please attribute credit
    /// Version 1.2 - 01.05.2013 by Salvador Richter - extended by ISensor and cm function
    /// </summary>
    public class HC_SR04 : ISensor
    {
        private OutputPort _outputPort;
        private InterruptPort _interruptInputPort;
        private long _beginTick;
        private long _endTick;
        private long _minTicks;  // System latency, subtracted off ticks to find actual sound travel time
        private int _waitDuration;
        private double _inchConversion;
        private double _cmConversion;
        private double _version;

        /// <summary>
        /// Default Constructor, call Init required for proper use
        /// </summary>
        public HC_SR04()
        {
        }

        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="pinTrig">Netduino pin connected to the HC-SR04 Trig pin</param>
        /// <param name="pinEcho">Netduino pin connected to the HC-SR04 Echo pin</param>
        public HC_SR04(Cpu.Pin pinEcho, Cpu.Pin pinTrig)
        {
            Init(pinEcho, pinTrig);
        }

        public void Init(Cpu.Pin PinToRead)
        {
            throw new NotImplementedException();
        }

        /// <summary>
        /// Init Method, required to call after use of default Constructor
        /// </summary>
        /// <param name="pinTrig">Netduino pin connected to the HC-SR04 Trig pin</param>
        /// <param name="pinEcho">Netduino pin connected to the HC-SR04 Echo pin</param>
        public void Init(Cpu.Pin pinEcho, Cpu.Pin pinTrig)
        {
            if (_outputPort == null)
            {
                _outputPort = new OutputPort(pinTrig, false);
                _interruptInputPort = new InterruptPort(pinEcho, false, Port.ResistorMode.Disabled, Port.InterruptMode.InterruptEdgeLow);
                _interruptInputPort.OnInterrupt += new NativeEventHandler(interIn_OnInterrupt);
                _minTicks = 6200L;
                _waitDuration = 50;
                _inchConversion = 1440.0;
                _cmConversion = 567;
                _version = 1.2;
            }
        }

        /// <summary>
        /// Returns the library _version number
        /// </summary>
        public double Version
        {
            get
            {
                return _version;
            }
        }

        public string GetSensorType()
        {
            return "ultrasonic";
        }

        public string GetDescription()
        {
            return "HC-SR04 ultrasonic range detector";
        }

        /// <summary>
        /// Returns the library _version number
        /// </summary>
        public int WaitDuration
        {
            get
            {
                return _waitDuration;
            }
            set
            {
                _waitDuration = value;
            }
        }

        /// <summary>
        /// Trigger a sensor reading
        /// Convert ticks to distance using TicksToInches below
        /// </summary>
        /// <returns>Number of ticks it takes to get back sonic pulse</returns>
        public long Ping()
        {
            int waitDuration = _waitDuration;
            int waitSlice = _waitDuration / 10;
            // Reset Sensor
            _outputPort.Write(true);
            Thread.Sleep(1);

            // Start Clock
            _endTick = 0L;
            _beginTick = System.DateTime.Now.Ticks;
            // Trigger Sonic Pulse
            _outputPort.Write(false);

            while (_endTick == 0L && waitDuration > 0)
            {
                // Wait max _waitDuration, return earlier if Value measured
                Thread.Sleep(waitSlice);
                waitDuration -= waitSlice;
            }

            if (_endTick > 0L)
            {
                // Calculate Difference
                long elapsed = _endTick - _beginTick;

                // Subtract out fixed overhead (interrupt lag, etc.)
                elapsed -= _minTicks;
                if (elapsed < 0L)
                {
                    elapsed = 0L;
                }

                // Return elapsed ticks
                return elapsed;
            }

            // Sonic pulse wasn't detected within waitDuration
            return -1L;
        }

        /// <summary>
        /// This interrupt will trigger when detector receives back reflected sonic pulse       
        /// </summary>
        /// <param name="data1">Not used</param>
        /// <param name="data2">Not used</param>
        /// <param name="time">Transfer to _endTick to calculated sound pulse travel time</param>
        void interIn_OnInterrupt(uint data1, uint data2, DateTime time)
        {
            // Save the ticks when pulse was received back
            _endTick = time.Ticks;
        }

        /// <summary>
        /// Convert ticks to inches
        /// </summary>
        /// <param name="ticks"></param>
        /// <returns></returns>
        public double TicksToInches(long ticks)
        {
            return (double)ticks / _inchConversion;
        }

        /// <summary>
        /// Convert ticks to cm
        /// </summary>
        /// <param name="ticks"></param>
        /// <returns></returns>
        public double TicksToCm(long ticks)
        {
            return (double)ticks / _cmConversion;
        }

        /// <summary>
        /// Returns distance measured in cm
        /// </summary>
        /// <param name="ticks"></param>
        /// <returns>Distance oin cm, -1L if measurement was out of range</returns>
        public double GetDistance()
        {
            return TicksToCm(Ping());
        }

        /// <summary>
        /// The ticks to inches conversion factor
        /// </summary>
        public double InchCoversionFactor
        {
            get
            {
                return _inchConversion;
            }
            set
            {
                _inchConversion = value;
            }
        }

        /// <summary>
        /// The system latency (minimum number of ticks)
        /// This number will be subtracted off to find actual sound travel time
        /// </summary>
        public long LatencyTicks
        {
            get
            {
                return _minTicks;
            }
            set
            {
                _minTicks = value;
            }
        }

        public void Dispose()
        {
            _outputPort.Dispose();
            _interruptInputPort.Dispose();
        }
    }

    public class Program
    {
        public static void Main()
        {
            HC_SR04 sensor = new HC_SR04(Pins.GPIO_PIN_D6, Pins.GPIO_PIN_D7);

            while (true)
            {
                long ticks = sensor.Ping();
                if (ticks > 0L)
                {
                    double inches = sensor.TicksToInches(ticks);
                }
            }
        }
    }
}